{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "ERSps3-ac4Po"
   },
   "source": [
    "# Introduction to QP"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "VsbCH_XUJDCN"
   },
   "source": [
    "## Notebook Setup \n",
    "The following cell will install Drake, checkout the manipulation repository, and set up the path (only if necessary).\n",
    "- On Google's Colaboratory, this **will take approximately two minutes** on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  \n",
    "\n",
    "More details are available [here](http://manipulation.mit.edu/drake.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "l3cuy181WKu6"
   },
   "outputs": [],
   "source": [
    "import importlib\n",
    "import os, sys\n",
    "from urllib.request import urlretrieve\n",
    "\n",
    "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
    "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
    "                \"setup_manipulation_colab.py\")\n",
    "    from setup_manipulation_colab import setup_manipulation\n",
    "    setup_manipulation(manipulation_sha='dc0eb8aa32e271d9b5299a84435694d0565346af', drake_version='20200909', drake_build='nightly')"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "tFMmTfbHWQfh"
   },
   "outputs": [],
   "source": [
    "# python libraries\n",
    "import numpy as np\n",
    "\n",
    "from pydrake.all import (\n",
    "    MathematicalProgram, Solve, eq, le, ge\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "ze9gQeOVOJUA"
   },
   "source": [
    "# Introduction to MathematicalProgram \n",
    "\n",
    "The purpose of this exercise is to get you familiar with the basics of what an instance of an optimization problem is, as well as how to solve it. \n",
    "\n",
    "An optimization problem is usually written as \n",
    "$$\\begin{aligned}\n",
    "\\min_x \\quad & f(x) \\\\\n",
    "\\textrm{s.t.} \\quad & g(x)\\leq 0,\\\\\n",
    "\\quad &  h(x)=0 \\end{aligned}$$\n",
    "\n",
    "We call $x$ the **decision variable**, $f(x)$ the **cost function**, $g(x)\\leq 0$ an **inequality constraint**, and $h(x)=0$ an **equality constraint**. We usually denote the optimal solution by $x^*$. Most of the times, the constraints are hard-constraints, meaning that they must be fulfilled by the optimal solution. \n",
    "\n",
    "Drake offers a very good interface to many solvers using `MathematicalProgram`. Let's try to solve a simple problem using `MathematicalProgram`: \n",
    "$$\\begin{aligned}\n",
    "\\min_x \\quad & \\frac{1}{2}x^2 \\\\\n",
    "\\textrm{s.t.} \\quad & x\\geq 3\n",
    "\\end{aligned}$$\n",
    "\n",
    "Before we start coding, what do you expect the answer to be? You should persuade yourself that the optimal solution is $x^*=3$, since that is value at which minimum cost is achieved without violating the constraint.\n",
    "\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "Khi7GeVNcwtU"
   },
   "outputs": [],
   "source": [
    "'''\n",
    "Steps to solve a optimization problem using Drake's MathematicalProgram\n",
    "'''\n",
    "# 1. Define an instance of MathematicalProgram \n",
    "prog = MathematicalProgram() \n",
    "\n",
    "# 2. Add decision varaibles\n",
    "x = prog.NewContinuousVariables(1)\n",
    "\n",
    "# 3. Add Cost function \n",
    "prog.AddCost(x.dot(x))\n",
    "\n",
    "# 4. Add Constraints\n",
    "prog.AddConstraint(x[0] >= 3)\n",
    "\n",
    "# 5. Solve the problem \n",
    "result = Solve(prog)\n",
    "\n",
    "# 6. Get the solution\n",
    "if (result.is_success): \n",
    "  print(\"Solution: \" + str(result.GetSolution()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "HvEI7697UUZC"
   },
   "source": [
    "You should have seen that we were successful in getting the expected solution of $x^*=3$. \n",
    "\n",
    "A particular class of problems that we want to focus on this problem are [Quadratic Programs (QP)](https://en.wikipedia.org/wiki/Quadratic_programming), which can be solved very efficiently in practice (even on the order of kHz).\n",
    "\n",
    "The general formulation of these problems are defined as follows. \n",
    "$$\\begin{aligned}\n",
    "\\min_x \\quad & \\frac{1}{2}x^T\\mathbf{Q}x + c^Tx \\\\\n",
    "\\textrm{s.t.} \\quad & \\mathbf{A}x\\leq b,\\\\\n",
    "\\quad &  \\mathbf{A}'x=b' \\end{aligned}$$\n",
    "\n",
    "where $\\mathbf{Q}$ is a positive-definite, symmetric matrix. Note that the cost is a quadratic function of the decision variables, while the constraints are all linear. This is what defines a convex QP. \n",
    "\n",
    "Let's practice solving a simple QP: \n",
    "\n",
    "$$\\begin{aligned}\n",
    "\\min_{x_0,x_1,x_2} \\quad & x_0^2 + x_1^2 + x_2^2 \\\\\n",
    "\\textrm{s.t.} \\quad & \\begin{pmatrix} 2 & 3 & 1 \\\\ 5 & 1 & 0 \\end{pmatrix} \\begin{pmatrix} x_0 \\\\ x_1 \\\\ x_2 \\end{pmatrix} = \\begin{pmatrix} 1 \\\\ 1 \\end{pmatrix}\\\\ \n",
    "\\quad &  \\begin{pmatrix} x_0 \\\\ x_1 \\\\ x_2 \\end{pmatrix} \\leq \\begin{pmatrix} 2 \\\\ 2 \\\\ 2\\end{pmatrix} \\end{aligned}$$\n",
    "\n",
    "To conveniently write down constraints that are vector-valued, Drake offers `eq,le,ge` for elementwise constraints. It might take some time to learn the syntax of constraints. For a more well-written and in-depth introduction to `MathematicalProgram`, [this notebook tutorial](https://mybinder.org/v2/gh/RobotLocomotion/drake/nightly-release-binder?filepath=tutorials/mathematical_program.ipynb) is incredibly useful. \n",
    " \n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "SNvpjgzxVQJC"
   },
   "outputs": [],
   "source": [
    "prog = MathematicalProgram()\n",
    "\n",
    "x = prog.NewContinuousVariables(3)\n",
    "\n",
    "prog.AddCost(x.dot(x)) \n",
    "prog.AddConstraint(eq(np.array([[2, 3, 1], [5, 1, 0]]).dot(x), [1, 1]))\n",
    "prog.AddConstraint(le(x, 2 * np.ones(3)))\n",
    "\n",
    "result = Solve(prog)\n",
    "\n",
    "# 6. Get the solution\n",
    "if (result.is_success()): \n",
    "  print(\"Solution: \" + str(result.GetSolution()))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "SmYZWSewSwf6"
   },
   "source": [
    "\n",
    "**Now, it's your turn to solve a simple problem!** \n",
    "\n",
    "You must solve the following problem and store the result in a variable named `result_submission`. \n",
    "\n",
    "$$\\begin{aligned}\n",
    "\\min_{x_0,x_1,x_2} \\quad & 2x_0^2 + x_1^2 + 3x_2^2 \\\\\n",
    "\\textrm{s.t.} \\quad & \\begin{pmatrix} 1 & 2 & 3 \\\\ 2 & 7 & 4 \\end{pmatrix} \\begin{pmatrix} x_0 \\\\ x_1  \\\\ x_2 \\end{pmatrix} = \\begin{pmatrix} 1 \\\\ 1 \\end{pmatrix} \\\\\n",
    "\\quad &  |x| \\leq \\begin{pmatrix} 0.35 \\\\ 0.35 \\\\ 0.35\\end{pmatrix} \\end{aligned}$$\n",
    "\n",
    "NOTE: The last constraint says that the absolute value of `x[i]` must be less than the value of `b_bb[i]`. You cannot put an absolute value directly as a constraint, so there are two routes that you can take:\n",
    "- Break the constraints down to two constraints that don't involve the absolute value.  \n",
    "- Drake offers [`AddBoundingboxConstraint`](https://drake.mit.edu/pydrake/pydrake.solvers.mathematicalprogram.html?highlight=addboundingbox#pydrake.solvers.mathematicalprogram.MathematicalProgram.AddBoundingBoxConstraint) which you may use in your implementation."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "qhMB4kc3asCE"
   },
   "outputs": [],
   "source": [
    "prog = MathematicalProgram() \n",
    "\n",
    "# Modify here to get the solution to the above optimization problem. \n",
    "\n",
    "result_submission = None # store the result here. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "zPmeRLtJk410"
   },
   "source": [
    "##How will this notebook be Graded?##\n",
    "\n",
    "If you are enrolled in the class, this notebook will be graded using [Gradescope](www.gradescope.com). You should have gotten the enrollement code on our announcement in Piazza. \n",
    "\n",
    "For submission of this assignment, you must do as follows:\n",
    "- Download and submit the notebook `intro_to_qp.ipynb` to Gradescope's notebook submission section, along with your notebook for the other problems.\n",
    "\n",
    "We will evaluate the local functions in the notebook to see if the function behaves as we have expected. For this exercise, the rubric is as follows:\n",
    "- [4 pts] `result_submission` must have the correct answer to the QP. "
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {
    "colab_type": "text",
    "id": "t4GLP2woecl5"
   },
   "source": [
    "Below is our autograder where you can check the correctness of your implementations. "
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {
    "colab": {},
    "colab_type": "code",
    "id": "Ea4zI6Enefhx"
   },
   "outputs": [],
   "source": [
    "from manipulation.exercises.pick.test_simple_qp import TestSimpleQP \n",
    "from manipulation.exercises.grader import Grader \n",
    "\n",
    "Grader.grade_output([TestSimpleQP], [locals()], 'results.json')\n",
    "Grader.print_test_results('results.json')"
   ]
  }
 ],
 "metadata": {
  "celltoolbar": "Tags",
  "colab": {
   "collapsed_sections": [],
   "name": "intro_to_qp.ipynb",
   "provenance": []
  },
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 1
}